home *** CD-ROM | disk | FTP | other *** search
/ Language/OS - Multiplatform Resource Library / LANGUAGE OS.iso / cpp_libs / cool / ge_cool.lha / GE_COOL2.1 / src / Transform / Quaternion.C < prev    next >
C/C++ Source or Header  |  1992-07-14  |  9KB  |  251 lines
  1. //
  2. // Copyright (C) 1992 General Electric Company.
  3. //
  4. // Permission is granted to any individual or institution to use, copy, modify,
  5. // and distribute this software, provided that this complete copyright and
  6. // permission notice is maintained, intact, in all copies and supporting
  7. // documentation.
  8. //
  9. // General Electric Company,
  10. // provides this software "as is" without express or implied warranty.
  11. //
  12. // Created: VDN 06/23/92 -- design and implementation
  13. //
  14. // Rep Invariant: 
  15. //   1. norm = 1, for a rotation.
  16. //   2. position vector represented by imaginary quaternion.
  17.  
  18.  
  19. #include <cool/Quaternion.h>
  20.  
  21. #include <cool/Matrix.C>
  22. #include <cool/M_Vector.C>
  23.  
  24. IMPLEMENT CoolMatrix<float>;
  25. IMPLEMENT CoolM_Vector<float>;
  26.  
  27.  
  28. // Quaternion -- Construct Quaternion from the 4 components
  29. // Input:      3 imaginary and 1 real components
  30. // Ouput:      none.
  31.  
  32. CoolQuaternion::CoolQuaternion (float x, float y, float z, float r) 
  33. : CoolM_Vector<float>(4) {
  34.   this->data[0] = x;                // 3 first elmts are 
  35.   this->data[1] = y;                // imaginary parts
  36.   this->data[2] = z;
  37.   this->data[3] = r;                // last element is real part
  38. }  
  39.  
  40. // Quaternion -- Construct Quaternion from angle and axis of rotation.
  41. // Input:      angle in radians, and 3D normalized vector for axis of rotation.
  42. // Ouput:      none.
  43.  
  44. CoolQuaternion::CoolQuaternion (const CoolM_Vector<float>& axis, float angle)
  45. : CoolM_Vector<float>(4) {
  46.   double a = angle / 2.0;            // half angle
  47.   double s = sin(a);
  48.   CoolM_Vector<float>& axis2 = (CoolM_Vector<float>&) axis; // cast away const
  49.   for (int i = 0; i < 3; i++)            // imaginary vector is sin of
  50.     this->data[i] = s * axis2(i);        // half angle multiplied with axis
  51.   this->data[3] = cos(a);            // real part is cos of half angle
  52. }
  53.  
  54.  
  55. // should be part of transform class.
  56.  
  57. CoolMatrix<float>E extract_3d_rotation (const CoolMatrix<float>& transform) {
  58.   if (transform.rows() != transform.columns()) {
  59.     printf("Ambiguous rotation submatrix from a %dx%d transform.\n", 
  60.        transform.rows(), transform.columns());
  61.     abort();
  62.   }
  63.   if (transform.rows() >= 3)
  64.     return transform.extract(3, 3, 0, 0);    // rotation is top-left most block
  65.   else {
  66.     CoolMatrix<float> rot(3, 3, 0.0);
  67.     CoolMatrix<float>& transform2 = (CoolMatrix<float>&) transform;
  68.     for (int i = 0; i < 2; i++)            // extract rotation in xy-plane
  69.       for (int j = 0; j < 2; j++)        // from transform.
  70.     rot(i,j) = transform2(i,j);
  71.     rot(2,2) = 1.0;                // zz component 
  72.     CoolMatrix<float>E& result = *((CoolMatrix<float>E*) &rot);
  73.     return result;                // avoid deep copy with envelope
  74.   }
  75. }
  76.  
  77. // Quaternion -- Construct Quaternion from 2-4 square row-major transform.
  78. //              Basis vectors is stored row-wise in submatrix(3,3,0,0).
  79. // Input:       Transformation matrix, with orthonormal basis vectors.
  80. // Output:      none.
  81.  
  82. CoolQuaternion::CoolQuaternion (const CoolMatrix<float>& transform) 
  83. : CoolM_Vector<float>(4) {
  84.   CoolMatrix<float> rot = extract_3d_rotation(transform);
  85.   double d0 = rot(0,0), d1 = rot(1,1), d2 = rot(2,2);
  86.   double xx = 1.0 + d0 - d1 - d2;        // from the diagonal of rotation
  87.   double yy = 1.0 - d0 + d1 - d2;        // matrix, find the terms in
  88.   double zz = 1.0 - d0 - d1 + d2;        // each Quaternion compoment
  89.   double rr = 1.0 + d0 + d1 + d2;
  90.  
  91.   double max = rr;                // find the maximum of all
  92.   if (xx > max) max = xx;                // diagonal terms.
  93.   if (yy > max) max = yy;
  94.   if (zz > max) max = zz;
  95.  
  96.   if (rr == max) {
  97.     double r4 = sqrt(rr * 4.0);
  98.     this->x() = (rot(1,2) - rot(2,1)) / r4;    // find other components from
  99.     this->y() = (rot(2,0) - rot(0,2)) / r4;    // off diagonal terms of
  100.     this->z() = (rot(0,1) - rot(1,0)) / r4;    // rotation matrix.
  101.     this->r() = r4 / 4.0;
  102.   } else if (xx == max) {
  103.     double x4 = sqrt(xx * 4.0);
  104.     this->x() = x4 / 4.0;
  105.     this->y() = (rot(0,1) + rot(1,0)) / x4;
  106.     this->z() = (rot(0,2) + rot(2,0)) / x4;
  107.     this->r() = (rot(1,2) - rot(2,1)) / x4;
  108.   } else if (yy == max) {
  109.     double y4 = sqrt(yy * 4.0);
  110.     this->x() = (rot(0,1) + rot(1,0)) / y4;
  111.     this->y() =  y4 / 4.0;
  112.     this->z() = (rot(1,2) + rot(2,1)) / y4;
  113.     this->r() = (rot(2,0) - rot(0,2)) / y4;
  114.   } else {
  115.     double z4 = sqrt(zz * 4.0);
  116.     this->x() = (rot(0,2) + rot(2,0)) / z4;
  117.     this->y() = (rot(1,2) + rot(2,1)) / z4;
  118.     this->z() =  z4 / 4.0;
  119.     this->r() = (rot(0,1) - rot(1,0)) / z4;
  120.   }
  121. }
  122.  
  123. // angle -- Positive angle of rotation
  124.  
  125. float CoolQuaternion::angle () const {
  126.   double sin = 0;
  127.   for (int i = 0; i < 3; i++)            // compute sin of half angle
  128.     sin += this->data[i] * this->data[i];    // from imaginary vector
  129.   sin = sqrt(sin);
  130.   double cos = this->data[3];            // cos of half angle
  131.   return (2.0 * atan2 (sin, cos));        // angle is always positive
  132. }
  133.  
  134. // axis -- Normalized direction vector of axis of rotation.
  135. //         Returns (0, 0, 1) if Quaternion is zero, and axis is not well defined.
  136.  
  137. CoolM_Vector<float>E CoolQuaternion::axis () const {
  138.   CoolM_Vector<float> dir = this->imaginary(); // dir parallel to imag. part
  139.   float mag = dir.magnitude();
  140.   if (mag == 0) {
  141.     cout << "Axis is not well defined for zero Quaternion. Use (0,0,1) instead. "
  142.       << endl;
  143.     dir.z() = 1.0;                // or signal exception here.
  144.   } else 
  145.     dir /= mag;                    // normalize direction vector
  146.   CoolM_Vector<float>E& result = *((CoolM_Vector<float>E*) &dir);
  147.   return result;                // avoid deep copy
  148. }
  149.  
  150.  
  151. // operator== -- Components of Quaternion are compared with fuzz = 1.0e-6
  152.  
  153. Boolean CoolQuaternion::operator== (const CoolQuaternion& rhs) const {
  154.   for (int i = 0; i < 4; i++)
  155.     if (fabs(this->data[i] - rhs.data[i]) > 1.0e-6) // more fuzz because of
  156.       return FALSE;                    // sqrt, etc...
  157.   return TRUE;
  158. }
  159.  
  160.  
  161. // rotation_transform -- Convert to a rotation transform matrix with 
  162. //            specified rows & cols. Assumed normalized Quaternion.
  163. // Input:     number of rows and columns specifying format of transform.
  164. // Output:    matrix for rotation transform equivalent to Quaternion.
  165.  
  166. CoolMatrix<float>E CoolQuaternion::rotation_transform (int dim) const {
  167.   CoolMatrix<float> rot(dim, dim, 0.0);
  168.   CoolQuaternion q(*this);            
  169.   if (dim == 2) {
  170.     q.x() = q.y() = 0.0;            // find best approx rotation
  171.     q.normalize();                // along z-axis only.
  172.     double s = q.z(), c = q.r();
  173.     rot(0,0) = rot(1,1) = (c * c) - (s * s);
  174.     rot(0,1) = 2.0 * s * c;
  175.     rot(1,0) = -rot(0,1);
  176.   } 
  177.   if (dim >= 3) {
  178.     double x2 = q.x() * q.x();
  179.     double y2 = q.y() * q.y();
  180.     double z2 = q.z() * q.z();
  181.     double r2 = q.r() * q.r();
  182.     rot(0,0) = r2 + x2 - y2 - z2;        // fill diagonal terms
  183.     rot(1,1) = r2 - x2 + y2 - z2;
  184.     rot(2,2) = r2 - x2 - y2 + z2;
  185.     double xy = q.x() * q.y();
  186.     double yz = q.y() * q.z();
  187.     double zx = q.z() * q.x();
  188.     double rx = q.r() * q.x();
  189.     double ry = q.r() * q.y();
  190.     double rz = q.r() * q.z();
  191.     rot(0,1) = 2 * (xy + rz);            // fill off diagonal terms
  192.     rot(0,2) = 2 * (zx - ry);
  193.     rot(1,2) = 2 * (yz + rx);
  194.     rot(1,0) = 2 * (xy - rz);
  195.     rot(2,0) = 2 * (zx + ry);
  196.     rot(2,1) = 2 * (yz - rx);
  197.   }
  198.   if (dim == 4) rot(3,3) = 1.0;            // for homogeneous transform
  199.   CoolMatrix<float>E& result = *((CoolMatrix<float>E*) &rot);
  200.   return result;                // avoid deep copy with envelope
  201. }
  202.  
  203. // conjugate -- Conjugate of a Quaternion has same real part and 
  204. //            opposite imaginary part.
  205.  
  206. CoolQuaternionE CoolQuaternion::conjugate () const {
  207.   CoolQuaternion* self = (CoolQuaternion*) this;    // cast away const
  208.   CoolQuaternion conj(-self->x(), -self->y(), -self->z(), self->r());
  209.   CoolQuaternionE& result = *((CoolQuaternionE *) &conj); // avoid deep copy with
  210.   return result;                  // envelope
  211. }
  212.  
  213. // inverse -- Inverse Quaternion exists only for nonzero Quaternion.
  214. //            If Quaternion represents rotation, inverse is conjugate.
  215.  
  216. CoolQuaternionE CoolQuaternion::inverse () const {
  217.   CoolQuaternion inv = this->conjugate() / dot_product(*this, *this);
  218.   CoolQuaternionE& result = *((CoolQuaternionE *) &inv); // avoid deep copy with
  219.   return result;                  // envelope
  220. }
  221.  
  222. // operator* -- Multiplication of two Quaternions is not symmetric
  223. //            and has fewer operations than mult of orthonormal matrices 
  224. //            If object is rotated by r1, then by r2, then the composed
  225. //            rotation (r2 o r1) is represented by the Quaternion (q2 * q1),
  226. //            and by the matrix (m1 * m2). 
  227. //            Remember that matrix and vector are represented row-wise,
  228. //            and this reverses the composition order.
  229.  
  230. CoolQuaternionE operator* (const CoolQuaternion& q1, const CoolQuaternion& q2) {
  231.   float r1 = q1.real();                // real and img parts of args
  232.   float r2 = q2.real();        
  233.   CoolM_Vector<float> i1 = q1.imaginary();
  234.   CoolM_Vector<float> i2 = q2.imaginary();
  235.   float real = (r1 * r2) - dot_product(i1, i2);    // real&img of product
  236.   CoolM_Vector<float> img = (r1 * i2) + (r2 * i1) + cross_3d(i1, i2);
  237.   CoolQuaternion prod(img.x(), img.y(), img.z(), real); 
  238.   CoolQuaternionE& result = *((CoolQuaternionE *) &prod); // avoid deep copy with
  239.   return result;                  // envelope
  240. }
  241.  
  242. // rotate --  Transform 3D vector by rotation Quaternion
  243. //            
  244.  
  245. void CoolQuaternion::rotate (CoolM_Vector<float>& v) const {
  246.   float r = this->real();
  247.   CoolM_Vector<float> i = this->imaginary();
  248.   v += float(2 * r) * cross_3d(i, v) -        // fewer number of mults
  249.     float(2) * cross_3d(cross_3d(i, v), i);
  250. }
  251.